#include "RoboController.h"
#include "RoboDrive.h"
#include "RoboRoller.h"
#include "RoboClamp.h"
#include "RoboDevice.h"
#include <cmath>
using namespace vex;
namespace RoboController{
    /**
     * @brief 获取手柄轴体的输入值
     * @param _Axis 读取的轴体,example:Axis3
     * @return double 0 ~ 100
     */
    double GetAxisValue(vex::controller::axis _Axis){
        return _Axis.position()/100.0 ;
    }

    
    /**
     * @brief 通过按键开启线程
     * @param _CtrlButton 
     * @param callback 
     */
    void ControllerButtonTaskOn(vex::controller::button _CtrlButton,void(*callback)(void)){
        _CtrlButton.pressed(callback);
    }
    /**
     * @brief 手柄
     */
    const vex::controller::axis *VcAxis = &RoboDevice::Controller.Axis3;
    const vex::controller::axis *WcAxis = &RoboDevice::Controller.Axis1;                              //默认手柄控制  V （速度）为 3号轴（左摇杆前后）； W （角速度） 为 1号轴 （右摇杆左右）
    const vex::controller::button *RoboBrakeButton = &RoboDevice::Controller.ButtonB;                 //默认刹车按钮为 B 
    const vex::controller::button *RoboturnlButton = &RoboDevice::Controller.ButtonL1;                //默认左转90按钮为L1
    const vex::controller::button *RoboturnrButton = &RoboDevice::Controller.ButtonR1;                //默认左转90按钮为R1
    // const vex::controller::button RoboArmUpButton = RoboDevice::Controller.ButtonUp;                  //默认机械臂向上转到限位位置按钮为Up
    // const vex::controller::button RoboArmDownButton = RoboDevice::Controller.ButtonDown;              //默认机械臂向下转到滚筒位置按钮为Down
    const vex::controller::button *RoboRollerRollingButtonLeft = &RoboDevice::Controller.ButtonLeft;  //默认滚筒旋转按钮为Left
    const vex::controller::button *RoboRollerRollingButtonRight = &RoboDevice::Controller.ButtonRight;//默认滚筒旋转按钮为Right
    const vex::controller::button *RoboClampHeadButtonRight = &RoboDevice::Controller.ButtonUp;       //默认车前面的爪子改变状态按钮为Up
    const vex::controller::button *RoboClampBackButtonRight = &RoboDevice::Controller.ButtonDown;     //默认车后面的爪子改变状态按钮为Down
    /**************************************************************************************************************************************/
    /**
     *@brief 以下是手柄控制小车的移动
     */ 
    double Vcmax = 1.4 ;
    double Wcmax = 8.0 ;
    bool IsBrake = false;

    
    /**
     * @brief //左转90设定按钮函数  ： 重新设置刹车按钮为 _CtrlButton 
     * 
     */
    void RoboturnlCallbackFunction(void){                                       //左转90返回函数
        RoboDrive::Rotate(-45,0.6);
    }

    void RoboturnlDetect(vex::controller::button _CtrlButton){          //左转90设定按钮函数  ： 重新设置刹车按钮为 _CtrlButton       
        RoboturnrButton = &_CtrlButton;
        RoboController::ControllerButtonTaskOn(_CtrlButton, RoboturnlCallbackFunction);
    }
    /**
     * @brief //右转90设定按钮函数  ： 重新设置刹车按钮为 _CtrlButton 
     * 
     */
    void RoboturnrCallbackFunction(void){                                       //右转90返回函数
        RoboDrive::Rotate(45,0.6);
    }

    void RoboturnrDetect(vex::controller::button _CtrlButton){                   //右转90设定按钮函数  ： 重新设置刹车按钮为 _CtrlButton    
        RoboturnlButton = &_CtrlButton;
        RoboController::ControllerButtonTaskOn(_CtrlButton, RoboturnrCallbackFunction);
    }
    /**
     * @brief //刹车函数
     *
     * @param _CtrlButton//RoboBrakeDetect函数  ： 重新设置刹车按钮为 _CtrlButton
     */
    void RoboBrakeCallbackFunction(void){                                       //刹车返回函数
        IsBrake = true;
        RoboDrive::BrakeMotor();
        while((*RoboBrakeButton).pressing()){                                                                                                                                                                                                                                                                                                                                   
            wait(40,msec);
        } 
        IsBrake = false;
    }

    void RoboBrakeDetect(vex::controller::button _CtrlButton){ 
        RoboBrakeButton = &_CtrlButton;                  //RoboBrakeDetect函数  ： 重新设置刹车按钮为 _CtrlButton        
        RoboController::ControllerButtonTaskOn(_CtrlButton, RoboBrakeCallbackFunction);
    }//刹车分配按钮

    /**
     * @brief 左右轴控制小车的运动
     * 
     * 
     */
    int RoboMoveTaskBackgroundFunction(void){                                                   //轴控制小车移动的callback函数 
        bool IsStop = false;
        while(1){
            
            double Wc  = - Wcmax *  (exp(fabs(RoboController::GetAxisValue(*WcAxis)*2))-1)/6.39 ;      //摇杆向右时使Wc<0 , 由于RoboDrive函数传入 W +为左 ， -为右
            if(RoboController::GetAxisValue(*WcAxis)<0){
              Wc = -Wc;
            }
            // if(RoboController::GetAxisValue(*VcAxis)<0){
            //   Wc = -Wc;
            // }
            double Vc = Vcmax  * RoboController::GetAxisValue(*VcAxis); 
            Wc = Wc / (fabs(Vc) *3 + 1);
            Vc = Vc;
            //Vc = Vc *(1/(fabs(Vc) + 0.10*Wc));
            RoboDevice::Brain.Screen.setCursor(20,20);
            RoboDevice::Brain.Screen.clearLine();
            RoboDevice::Brain.Screen.setCursor(20,40);
            RoboDevice::Brain.Screen.clearLine();
            RoboDevice::Brain.Screen.setCursor(20,60);
            RoboDevice::Brain.Screen.clearLine();
            RoboDevice::Brain.Screen.printAt(20,20,"Vc     :%.3lf   Wc : %.3lf",Vc,Wc);
            RoboDevice::Brain.Screen.printAt(20,40,"IsBrake:%c   ",(IsBrake?('T'):('F')));
            RoboDevice::Brain.Screen.printAt(20,60,"IsStop :%c   ",(IsStop?('T'):('F')));
            if( -0.1<=Vc && Vc<=0.1 && -0.1<=Wc && Wc<=0.1){
                if(!IsStop){
                    RoboDrive::Drive(0,0);//由于原函数更改，差一个时间的输入量，填补了0      
                    IsStop = true;
                    wait(30,msec);
                    if(Vc>0)
                        RoboDrive::BrakeMotor_1();
                    else 
                        RoboDrive::BrakeMotor();
                }
                wait(30,msec);
                continue;
            }
            if(!IsBrake){
                RoboDrive::Drive(Vc,Wc);//由于原函数更改，差一个时间的输入量，填补了0.5  
                IsStop = false;
            }
            wait(30,msec);
        }
        return 0;
    }

    vex::task *RoboMoveTask = nullptr;
    void RoboMoveDetect(const vex::controller::axis _VcAxis,const vex::controller::axis _WcAxis){
        delete RoboMoveTask;
        VcAxis = &_VcAxis;
        WcAxis = &_WcAxis;
        RoboMoveTask = new vex::task(RoboMoveTaskBackgroundFunction);
    }

    /*
    ***********************************机械臂与滚筒*************************************
     * @brief 以下为机械臂手柄实现Up
     * 
     */
    // void RoboArmUpCallbackFunction(void){                                       //
       
    //     RoboRoller::ArmUpRollinginAngle(30/*需要测*/);
    //     while(!RoboDevice::RoboArm.isDone()){
    //         wait(0.05,sec);
    //     }
    //     RoboDevice::RoboArm.stop(coast);
    // }

    // void RoboArmUpDetect(vex::controller::button _CtrlButton){                   //函数  ：机械臂向上转到限位位置 重新设置为 _CtrlButton        
    //     RoboArmUpButton = _CtrlButton;
    //     RoboController::ControllerButtonTaskOn(RoboArmUpButton,RoboArmUpCallbackFunction);
    // }
    // /**
    //  * @brief 以下为机械臂手柄实现Down
    //  * 
    //  */
    // void RoboArmDownCallbackFunction(void){                                       //
       
    //     RoboRoller::ArmDownRollinginAngle(30/*需要测*/);
    //     while(!RoboDevice::RoboArm.isDone()){
    //         wait(0.05,sec);
    //     }
    //     RoboDevice::RoboArm.stop(coast);
    // }

    // void RoboArmDownDetect(vex::controller::button _CtrlButton){                   //函数  ：机械臂向下转到滚筒位置 重新设置为 _CtrlButton        
    //     RoboArmDownButton = _CtrlButton;
    //     RoboController::ControllerButtonTaskOn(RoboArmDownButton,RoboArmDownCallbackFunction);
    // }
    
    // /**
    //  * @brief 以下为滚筒转动手柄实现Left
    //  * 按下旋转，松开不旋转
    //  */
    void RoboRollerRollingCallbackFunctionRight(void){                                       
        RoboRoller::RollerRollingWithW(8);
        while((*RoboRollerRollingButtonRight).pressing()){                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             
            wait(30,msec);
        }
        RoboDevice::RoboRoller.stop(brake);
    }

    void RoboRollerRollingDetectRight(vex::controller::button _CtrlButton){                   //函数  ：滚筒旋转 重新设置为 _CtrlButton        
        RoboRollerRollingButtonRight = &_CtrlButton;
        RoboController::ControllerButtonTaskOn(_CtrlButton,RoboRollerRollingCallbackFunctionRight);
    }

    void RoboRollerRollingCallbackFunctionLeft(void){                                       
        RoboRoller::RollerRollingWithW(-8);
        while((*RoboRollerRollingButtonLeft).pressing()){
            wait(30,msec);
        }
        RoboDevice::RoboRoller.stop(brake);
    }

    void RoboRollerRollingDetectLeft(vex::controller::button _CtrlButton){       
        RoboRollerRollingButtonLeft = &_CtrlButton;            //函数  ：滚筒旋转 重新设置为 _CtrlButton        
        RoboController::ControllerButtonTaskOn(_CtrlButton,RoboRollerRollingCallbackFunctionLeft);
    }
    
    /**
     * @brief 夹子**********************************************************************************************************************
     * @
     */
    //前夹子
    bool headcatch = false; 
    int RoboClampHeadCallbackFunction(void){     
        
        if(headcatch == true){
            headcatch = false;
            RoboClamp::RoboClampHead_Release();
        }
        else{
            headcatch = true;
            RoboClamp::RoboClampHead_Lock();
        }
        return 0;
    }

    vex::task *RoboClampHeadTask = nullptr;
    void RoboClampHeadBackGroundFunction(void){
        delete RoboClampHeadTask;
        RoboClampHeadTask = new vex::task(RoboClampHeadCallbackFunction);
    }

    void RoboClampHeadDetect(vex::controller::button _CtrlButton){
        
        RoboClampHeadButtonRight = &_CtrlButton;
        RoboController::ControllerButtonTaskOn(_CtrlButton,RoboClampHeadBackGroundFunction);
    }
    
    //后夹子
    
    bool backcatch = false; 
    int RoboClampBackCallbackFunction(void){     
        
        if(backcatch == true){
            backcatch = false;
            RoboClamp::RoboClampBack_Release();
        }
        else{
            backcatch = true;
            RoboClamp::RoboClampBack_Lock();
        }
        return 0;
    }

    vex::task *RoboClampBackTask = nullptr;
    void RoboClampBackBackGroundFunction(void){
        delete RoboClampBackTask;
        RoboClampBackTask = new vex::task(RoboClampBackCallbackFunction);
    }

    void RoboClampBackDetect(vex::controller::button _CtrlButton){
        
        RoboClampBackButtonRight = &_CtrlButton;
        RoboController::ControllerButtonTaskOn(_CtrlButton,RoboClampBackBackGroundFunction);
    }



    


}

